#include "cluster.h"

#include <cmath>
#include <algorithm>

#include <matrix.h>

// Point - Point distances

double distance_rp(const Point& p, const Point& q, double d) {
  double r = 0.0;
  for (int i = 0; i < CH; ++i) {
    r += std::pow(p[i] - q[i], d);
  }
  return std::pow(r, 1 / d);
}

double distance_euclid(const Point& p, const Point& q) {
  double r = 0.0;
  for (int i = 0; i < CH; ++i) {
    r += (p[i] - q[i]) * (p[i] - q[i]);
  }
  return std::sqrt(r);
}

double distance_rinf(const Point& p, const Point& q) {
  double r = 0.0;
  for (int i = 0; i < CH; ++i) {
    r = std::max(std::fabs(p[i] - q[i]), r);
  }
  return r;
}

double distance_div(const Point& p, const Point& q) {
  double r = 0.0;
  for (int i = 0; i < CH; ++i) {
    r += std::pow((p[i] - q[i]) / (p[i] + q[i]), 2);
  }
  return std::pow(r / CH, 0.5);
}

double distance_jm(const Point& p, const Point& q) {
  double r = 0.0;
  for (int i = 0; i < CH; ++i) {
    r += std::pow(std::sqrt(p[i]) - std::sqrt(q[i]), 2);
  }
  return std::pow(r, 0.5);
}

double mon_distance_rp(const Point& p, const Point& q, double d) {
  double r = 0.0;
  for (int i = 0; i < CH; ++i) {
    r += std::pow(p[i] - q[i], d);
  }
  return r;
}

double mon_distance_euclid(const Point& p, const Point& q) {
  double r = 0.0;
  for (int i = 0; i < CH; ++i) {
    r += (p[i] - q[i]) * (p[i] - q[i]);
  }
  return r;
}

// Cluster - Cluster distances

double distance_avg(const Cluster& p, const Cluster& q) {
  double r = 0.0;
  for (int i = 0; i < 3; ++i) {
    r += (p._sqr[i][i] * q._size + q._sqr[i][i] * p._size 
	  - 2 * p._sum[i] * q._sum[i]) / (p._size * q._size);
  }
  return std::sqrt(r);
}

double distance_mah(const Cluster& p, const Cluster& q) {
  Matrix pm(CH, CH), qm(CH, CH);
  
  for (int i = 0; i < CH; ++i) {
    for (int j = 0; j < CH; ++j) {
      pm(i, j) = p.covariance(i, j);
      qm(i, j) = q.covariance(i, j);
    }
    pm(i, i) += 0.01;
    qm(i, i) += 0.01;
  }

  Vector dm(CH);
  for (int i = 0; i < CH; ++i) {
    dm(i) = p.expected(i) - q.expected(i);
  }

  return dm * (pm.inverse() + qm.inverse()) * dm;
}

double distance_div(const Cluster& p, const Cluster& q) {
  Matrix pm(CH, CH), qm(CH, CH);
  
  for (int i = 0; i < CH; ++i) {
    for (int j = 0; j < CH; ++j) {
      pm(i, j) = p.covariance(i, j);
      qm(i, j) = q.covariance(i, j);
    }
    pm(i, i) += 0.01;
    qm(i, i) += 0.01;
  }

  Vector dm(CH);
  for (int i = 0; i < CH; ++i) {
    dm(i) = p.expected(i) - q.expected(i);
  }

  Matrix pmi = pm.inverse();
  Matrix qmi = qm.inverse();

  return 0.5 * mulTrace(pm - qm, pmi - qmi) + dm * (pmi + qmi) * dm;
} 

double distance_bat(const Cluster& p, const Cluster& q) {
  Matrix pm(CH, CH), qm(CH, CH);
  
  for (int i = 0; i < CH; ++i) {
    for (int j = 0; j < CH; ++j) {
      pm(i, j) = p.covariance(i, j);
      qm(i, j) = q.covariance(i, j);
    }
    pm(i, i) += 0.01;
    qm(i, i) += 0.01;
  }

  Vector dm(CH);
  for (int i = 0; i < CH; ++i) {
    dm(i) = p.expected(i) - q.expected(i);
  }

  Matrix mm = 0.5 * (pm + qm);
  Matrix mmi = mm.inverse();

  return 0.125 * dm * mmi * dm
    + 0.5 * std::log(mm.determinant() 
		     / std::sqrt(pm.determinant() * qm.determinant()));
}

double distance_jm(const Cluster& p, const Cluster& q) {
  return std::sqrt(2 * std::exp(- distance_bat(p, q)));
}

double distance_stat(const Cluster& p, const Cluster& q) {
  double r = 0.0;
  for (int i = 0; i < CH; ++i) {
    r += std::pow(p.expected(i) - q.expected(i), 2);
  }
  return r * p.size() * q.size() / (p.size() + q.size());
}

// Cluster - Point distances

double distance_rp(const Cluster& p, const Point& q, double d) {
  return distance_rp(p.center(), q, d);
}

double distance_avg(const Cluster& p, const Point& q) {
  double r = 0.0;
  for (int i = 0; i < CH; ++i) {
    r += (p._sqr[i][i] -  2 * p._sum[i] * q.data[i] + 
	  p._size * q.data[i] * q.data[i]) / p._size;
  }
  return std::sqrt(r);
}

double mon_distance_avg(const Cluster& p, const Point& q) {
  double r = 0.0;
  for (int i = 0; i < CH; ++i) {
    r += (p._sqr[i][i] -  2 * p._sum[i] * q.data[i] + 
	  p._size * q.data[i] * q.data[i]) / p._size;
  }
  return r;
}

double inv_map_rp(double dist, double d) {
  return std::pow(dist, d);
}

double inv_map_euclid(double dist) {
  return dist * dist;
}
